#include <cmath>
#include <cstdio>
#include "rrtype.h"
#include "rrmath.h"

RRVec3 RRNormalize(RRVec3 arg)
{
    float len;
    RRVec3 v;
    len = sqrtf(arg.x * arg.x + arg.y * arg.y + arg.z * arg.z);
    v.x = arg.x / len;
    v.y = arg.y / len;
    v.z = arg.z / len;

    return v;
}

RRVec3 RRCrossProduct(RRVec3 v1, RRVec3 v2)
{
    RRVec3 r;
    r.x = v1.y * v2.z - v1.z * v2.y;
    r.y = v1.z * v2.x - v1.x * v2.z;
    r.z = v1.x * v2.y - v1.y * v2.x;

    return r;
}

float RRInnerProduct(RRVec3 v1, RRVec3 v2)
{
    return v1.x * v2.x + v1.y * v2.y + v1.z * v2.z;
}

void RRMatrixProduct3f(RRVec3 m1[], RRVec3 m2[], RRVec3 m[])
{
    for(int i=0; i<3; i++) {
        m[i].x = m1[0].x * m2[i].x + m1[1].x * m2[i].y + m1[2].x * m2[i].z;
        m[i].y = m1[0].y * m2[i].x + m1[1].y * m2[i].y + m1[2].y * m2[i].z;
        m[i].z = m1[0].z * m2[i].x + m1[1].z * m2[i].y + m1[2].z * m2[i].z;
    }
}

void RRMatVecProduct3f(RRVec3 m1[], RRVec3 m2, RRVec3 *m)
{
    m->x = m1[0].x * m2.x + m1[1].x * m2.y + m1[2].x * m2.z;
    m->y = m1[0].y * m2.x + m1[1].y * m2.y + m1[2].y * m2.z;
    m->z = m1[0].z * m2.x + m1[1].z * m2.y + m1[2].z * m2.z;
}

RRVec3 RRSetVec3(float x, float y, float z)
{
    RRVec3 t;
    t.x = x; t.y = y; t.z = z;
    return t;
}

void RRClampVec3(RRVec3 *arg, float low, float up)
{
    if(arg->x > up) arg->x = up;
    else if(arg->x < low) arg->x = low;
    if(arg->y > up) arg->y = up;
    else if(arg->y < low) arg->y = low;
    if(arg->z > up) arg->z = up;
    else if(arg->z < low) arg->z = low;
}

